/*
 * JSON串格式: {
 *                 "Name": "AbdominalMotor",
 *                 "MotorNum": 21,   // or 22, or 23, or 0
 *                 "Angle": (int)xxx
 *             }
 * 
 * 电机顺序(从上到下): 1, 2, 3 - 第二款机器人
 * 
 * Ctrl+C 后需要稍微等待一下，这段时间电机会自动运行至原点
 * 
 * 各函数功能可在MotorMoving() 中修改, 其余有关电机的函数请看 '../include/mode_chose.h' 和 './mode_chose.cpp' 
 * 
 * Motor 限位
 * Motor1       -40° <-> 40°
 * Motor2       -10° <-> 10°
 * Motor3       -20° <-> 20°
*/

#include "../include/eu_harmonic.h"
#include "../include/mode_chose.h"
#include <thread>
#include <iostream>
#include <csignal>

// 设置电机的角度限制
#define Motor1AngleLimit 40
#define Motor2AngleLimit 10
#define Motor3AngleLimit 20

int main(int argc, char *argv[]){
    int initdllstate;
    int SetOperateModeState[3];

    std::cout << std::endl;
    std::cout << "The sequence of abdominal motors from top to botton: 1, 2, 3" << std::endl;
    std::cout << "The rotation range of motor 1 is : -40 <-> 40" << std::endl;
    std::cout << "The rotation range of motor 2 is : -10 <-> 10" << std::endl;
    std::cout << "The rotation range of motor 3 is : -20 <-> 20" << std::endl << std::endl;

    // 初始化电机
    initdllstate = harmonic_initDLL(harmonic_DeviceType_Canable, canid, harmonic_Baudrate_1000);
    if(HARMONIC_SUCCESS != initdllstate){
        std::cout << "[Error]harmonic_initDLL: Test open failed!" << std::endl;
        std::cout << "[Error]harmonic_initDLL: initdllstate(int) is " << initdllstate << std::endl;
        std::cout << std::endl;
        harmonic_freeDLL(canid);
        return -1;
    }
    std::cout << std::endl << "[Note]harmonic_initDLL is completed!!!" << std::endl;

    for(int i = Motor0 +1; i <= Motor3; i++)
    SetOperateModeState[0] = harmonic_setOperateMode(canid, i, harmonic_OperateMode_ProfilePosition);
    if(HARMONIC_SUCCESS != SetOperateModeState[0]){
        std::cout << std::endl << "[Error]harmonic_setOperateMode: Test setOperateMode failed!" << std::endl;
        std::cout << "[Error]harmonic_setOperateMode: SetOperateModeState (int) is " << SetOperateModeState[0] << std::endl;
        std::cout << std::endl;

        harmonic_freeDLL(canid);
        return -1;
    }
    std::cout << std::endl << "[Note]harmonic_setOperateMode: Set operate mode complete!!" << std::endl << std::endl;

    // 注册 Ctrl+C 信号的处理程序
    // signal(SIGINT, signal_handler);
    for(int i = Motor0 +1; i <= Motor3; i++){
        MotorMoving(i, 10, Motor1AngleLimit);
        MotorMoving(i, -10, Motor1AngleLimit);
        MotorMoving(i, 0, Motor1AngleLimit);
    }

    std::cout << std::endl;
    harmonic_freeDLL(canid);
    return 0;
}